/*
    需求：编写参数客户端，获取或修改服务端参数。
    步骤：
        1.包含头文件；
        2.初始化 ROS2 客户端；
        3.定义节点类；
            3-1.创建一个参数客户端对象
            3-2.等待服务连接
            3-3.查询参数；
            3-4.修改参数；
        4.创建节点对象指针，调用参数操作函数；
        5.释放资源。
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"

using namespace std::chrono_literals;

// 3.定义节点类；
class MinimalParamClient: public rclcpp::Node {
public:
    MinimalParamClient():Node("paramDemoClient_node"){
        // 3-1.创建一个参数客户端对象
        // node 当前对象依赖的节点
        // remote_node_name 连接的服务端对象节点名
        // qos 通信质量服务策略选项
        param_client_ = std::make_shared<rclcpp::SyncParametersClient>(this,"minimal_param_server");
    }

    // 3-2.等待服务连接
    bool connect_server(){
        while(!param_client_->wait_for_service(1s)){
            if(!rclcpp::ok()){ //按下了crtl c 停止

                return false;
            }
            RCLCPP_INFO(this->get_logger(),"服务连接中");
        }
        return true;
    }

    // 3-3.查询参数；
    void get_param(){
        RCLCPP_INFO(this->get_logger(),"-----------参数客户端查询参数-----------");
        double height = param_client_->get_parameter<double>("height");
        RCLCPP_INFO(this->get_logger(),"height = %.2f", height);
        RCLCPP_INFO(this->get_logger(),"car_type 存在吗？%d", param_client_->has_parameter("car_type"));
        auto params = param_client_->get_parameters({"car_type","height","wheels"});
        for (auto &param : params)
        {
            RCLCPP_INFO(this->get_logger(),"%s = %s", param.get_name().c_str(),param.value_to_string().c_str());
        }
    }

    // 3-4.修改参数；
    void update_param(){
        RCLCPP_INFO(this->get_logger(),"-----------参数客户端修改参数-----------");
        param_client_->set_parameters({rclcpp::Parameter("car_type","Mouse"),
        rclcpp::Parameter("height",2.0),
        //这是服务端不存在的参数，只有服务端设置了rclcpp::NodeOptions().allow_undeclared_parameters(true)时，
        // 这个参数才会被成功设置。
        rclcpp::Parameter("width",0.15),
        rclcpp::Parameter("wheels",6)});
    }

private:
    //设置一个同步参数客户端对象
    rclcpp::SyncParametersClient::SharedPtr param_client_;
};

int main(int argc, char const *argv[])
{
    // 2.初始化 ROS2 客户端；
    rclcpp::init(argc,argv);

    // 4.创建节点对象指针，调用参数操作函数；
    auto paramClient = std::make_shared<MinimalParamClient>();
    //连接参数服务端
    bool flag = paramClient->connect_server();
    if(!flag){
        return 0;
    }
    paramClient->get_param();
    paramClient->update_param();
    paramClient->get_param();

    // 5.释放资源。
    rclcpp::shutdown();
    return 0;
}